#include "robot_one_navigation/A_Star/Astar_planner.hpp"

namespace astar_planner
{
    void AstarPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        node_ = parent.lock();
        name_ = name;
        tf_buffer_ = tf;
        cost_map_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();

        pub_vis_map_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>("astar_vis_map",
                                                                             rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
    }

    void AstarPlanner::cleanup()
    {
        pub_vis_map_.reset();
        RCLCPP_INFO(node_->get_logger(), "正在清除%s插件!", name_.c_str());
    }

    void AstarPlanner::activate()
    {
        pub_vis_map_->on_activate();
        RCLCPP_INFO(node_->get_logger(), "正在激活%s插件!", name_.c_str());
    }

    void AstarPlanner::deactivate()
    {
        pub_vis_map_->on_deactivate();
        RCLCPP_INFO(node_->get_logger(), "正在反激活%s插件!", name_.c_str());
    }

    nav_msgs::msg::Path AstarPlanner::createPlan(
        const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal)
    {
        std::chrono::steady_clock::time_point time_point_start = std::chrono::steady_clock::now();

        // 获取代价地图大小信息
        astar_.set_costmap(cost_map_);

        nav_msgs::msg::Path plan;
        plan.poses.clear();
        plan.header.stamp = node_->now();
        plan.header.frame_id = global_frame_;

        // 边界检查
        if (start.header.frame_id != global_frame_ || goal.header.frame_id != global_frame_)
        {
            RCLCPP_ERROR(node_->get_logger(), "规划器只接受%s中的起点与终点", global_frame_.c_str());
            return plan;
        }
        // world->map
        unsigned int mx_start, my_start, mx_goal, my_goal;
        cost_map_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
        cost_map_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);

        std::vector<Eigen::Vector2i> grid_path;
        if (astar_.create_path(Eigen::Vector2i(mx_start, my_start), Eigen::Vector2i(mx_goal, my_goal), grid_path) && !grid_path.empty())
        {
            for (size_t i = 0; i < grid_path.size(); ++i)
            {
                geometry_msgs::msg::PoseStamped pose;
                pose.header.stamp = node_->now();
                pose.header.frame_id = global_frame_;
                pose.pose = get_world_coords(grid_path[i], cost_map_);
                pose.pose.position.z = 0.0;

                if (i + 1 < grid_path.size())
                {
                    auto next = get_world_coords(grid_path[i + 1], cost_map_);
                    double dx = next.position.x - pose.pose.position.x;
                    double dy = next.position.y - pose.pose.position.y;
                    double yaw = std::atan2(dx, dy);
                    tf2::Quaternion q;
                    q.setRPY(0, 0, yaw);
                    pose.pose.orientation = tf2::toMsg(q);
                }
                else
                {
                    pose.pose.orientation = goal.pose.orientation;
                }

                plan.poses.push_back(pose);
            }
        }
        nav_msgs::msg::OccupancyGrid search_vis = astar_.visualize();
        search_vis.header.stamp = node_->now();
        pub_vis_map_->publish(search_vis);

        // 记录运行时间
        std::chrono::steady_clock::time_point time_point_end = std::chrono::steady_clock::now();
        std::chrono::duration<double> time_span =
            std::chrono::duration_cast<std::chrono::duration<double>>(time_point_end - time_point_start);
        std::cout << "Astar算法运行时间：" << time_span.count() * 1000 << "ms" << std::endl;

        return plan;
    }

    // 将 costmap 栅格地图中的二维栅格坐标（行列索引）转换为世界坐标系下的三维位姿（Pose）
    geometry_msgs::msg::Pose AstarPlanner::get_world_coords(const Eigen::Vector2i &pose,
                                                            const nav2_costmap_2d::Costmap2D *costmap)
    {
        geometry_msgs::msg::Pose msg;
        msg.position.x = static_cast<float>(costmap->getOriginX()) + (static_cast<float>(pose(0)) + 0.5) * costmap->getResolution();
        msg.position.y = static_cast<float>(costmap->getOriginY()) + (static_cast<float>(pose(1)) + 0.5) * costmap->getResolution();
        return msg;
    }

} // namespace astar_planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(astar_planner::AstarPlanner, nav2_core::GlobalPlanner)